#include "usb_device.h"
#include "odrive_main.h"

#include <cmsis_os.h>
#include <freertos_vars.h>

osThreadId usb_thread;
const uint32_t stack_size_usb_thread = 4096; // Bytes
uint8_t send_data[10][64];
uint8_t add_ptr=0,
        send_ptr=0;
static void usb_server_thread(void * ctx) {
    (void) ctx;

    while(1){
        if (USBD_CONFIGURED == cdc_acm.dev.cur_status) {
            if (0U == cdc_acm_check_ready(&cdc_acm)) {
                cdc_acm_data_receive(&cdc_acm);
            } else {
                usb_cdc_handler *cdc = (usb_cdc_handler *)cdc_acm.dev.class_data[CDC_COM_INTERFACE];
                cdc->packet_sent = 0U;
                if(send_ptr!=add_ptr)
                    usbd_ep_send (&cdc_acm, CDC_DATA_IN_EP, send_data[send_ptr++], cdc->receive_length);
                if(send_ptr==10)
                    send_ptr=0;
                cdc->receive_length = 0U;
                
            }
        }
        osDelay(10);
    }
}

void start_usb_server() {
    // Start USB communication thread
    osThreadDef(usb_server_thread_def, usb_server_thread, osPriorityNormal, 0, stack_size_usb_thread / sizeof(StackType_t));
    usb_thread = osThreadCreate(osThread(usb_server_thread_def), NULL);
}


/* int _write(int file, char *ptr, int len)
{
    uint16_t todo = len;   
    for(;todo>64;todo-=64,ptr+=64){
        memcpy(send_data[add_ptr++],ptr,64);
        if(add_ptr==10)
            add_ptr=0;
    }
    return len;
} */